;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;;;          strict-inequality constrained optimization(带严格不等式约束的最优化问题)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;; Problem:
;;               min f(x) where x in R^n and c_i(x) > 0, i=1,..,I.

;; Framework: construct a seq of x: x0, x1 = x0 + s0 * d0, x2 = x1 + s1 * d1, ...

;; Key point: the allowed region of points is a **open set**.


#!r6rs
(library-directories '("." "/home/cl/gits/libs-for-chezscheme"))
(import (linear-algebra)
        (stream)
        (range)
        (gnuplot))


;;next-point
(define (next-point x s d) (vector-add x (scalar-*-vector s d)))        

(define E (exp 1))

;; get-step function
(define (get-step f grad constraints grad-constrs sigma epsilon-x c1 c2)                     ;get-step
  ;; objective-function: f(x), grad(x): grad of f, constraints(x): constraint-functions(a list)
  ;; sigma is used into construct potential-barrier by constraints.
  ;; epsilon-x: minimun of distance of two neighbor points
  ;; return: (cons s new-sigma)
  (lambda (x d default-step)
    (define (in-region? s)
      (allowed-point? (next-point x s d) constraints))
    (define (y s)
      (let ([xs (next-point x s d)])
        (if (zero? sigma) (f xs)
            (- (f xs) (* sigma (apply + (map (lambda (c) (log c E)) (constraints xs))))))))
    ;;须确保上面的 c > 0 
    (define (dy s)
      (let ([xs (next-point x s d)])
        (if (zero? sigma) (vector-dot d (grad xs))
            (- (vector-dot d (grad xs))
               (* sigma (apply + (map (lambda (c g) (/ (vector-dot d g) c))
                                      (constraints xs) (grad-constrs xs))))))))
    (if default-step
        (line-search-algorithm y dy in-region? sigma default-step 'any c1 c2)   ;line-search-algorithm
        (let ([min-step (* 10 (/ epsilon-x (vector-norm d)))])
          (line-search-algorithm y dy in-region? sigma #f min-step c1 c2)))))

;; allowed-point?
(define (allowed-point? x constraints)
  (let loop ([ls (constraints x)])
    (if (null? ls) #t
        (if (positive? (car ls)) (loop (cdr ls)) #f)))) ;;可改进

                                        ; Armijo condition (ac?)
(define (armijo-condition? c1)
  (lambda (y0 y1 dy0 s)
    (<= y1 (+ y0 (* c1 s dy0)))))
                                        ; curvature condition (cc?)
(define (curvature-condition? c2)
  (lambda (dy0 dys)
    (<= (abs dys) (* c2 (abs dy0)))))

;; --- line search algorithm
(define (line-search-algorithm y dy in-region? sigma default-step min-step c1 c2)
  ;; assume: 0 < c1 < c2 <1, dy(0) < 0
  ;; if default-step is given, use default-step, else use min-step to construct step-size
  ;; return: (cons s new-sigma) where some s in-region that satisfies strong-wolfe-rule.
  (let ([y0 (y 0)] [dy0 (dy 0)])
    (define (sdc? s s0)                     ;sdc?(sufficient-desceant-condition, extend ac?)
      (lambda (ys ys0)
        (and ((armijo-condition? c1) y0 ys dy0 s) (<= ys ys0))))
    (define (cc? s)                         ;cc?
      (lambda (dys)
        ((curvature-condition? c2) dy0 dys)))
    (define (make-step-allowed s pre-s)               ;make-step-allowed
      (let ([touch #f])
        (let loop ([s s])
          (if (in-region? s)
              (cons s touch) ;;return
              (begin ;;(printf " --touch-constraint ~a--\n " sigma)
                     (set! touch #t)
                     (loop (/ (+ s pre-s) 2) ))))))
    (define step-size                       ;step-size
      (if default-step default-step
          (let ([ddy0 (/ (- (dy  min-step) dy0) min-step)])
            (if (positive? ddy0) (- (/ dy0 ddy0)) min-step))))
    (define (increase-step s) (make-step-allowed (+ s step-size) s))  ;increase-step
    
    ;;ZOOM function 
    (define (zoom current bound)
      ;; if the allowed-region is convex, then the inner point of current,bound is also in allowed-region
      (lambda (yc dyc yb) ;c=current,b=bound
        (assert (in-region? current))
        (assert (in-region? bound))
        ;;(printf "zoom: ~a, ~a ****\n " current bound)
        (let* ([s (inner-point current bound yc dyc yb)]
               [ys (y s)]
               [dys (dy s)])
          (assert (in-region? s))
          (if ((sdc? s current) ys yc)
              (cond [((cc? s) dys) s] ;ok
                    [(negative? (* dys (- bound current))) ;;(display " here!*****")
                     ((zoom s bound) ys dys yb)]
                    (else ((zoom s current) ys dys yc)))
              (begin ;;(display " sdc-no----- ")
                     ((zoom current s) yc dyc ys))))))
    
    (define (inner-point c b yc dyc yb)           ; inner-point between c and b
      (let ([aux (/ (+ b c) 2 (- 1 (/ (- yb yc) (- b c) dyc)))])
        (if (or (nan? aux) (<= (* (- aux c) (- b aux)) 0))
            (/ (+ b c) 2)  aux)))
    
    
    ;; MAIN function
    ;; loop to get an increasing seq: s0 = 0 < s1 < s2 < ..., until one can satisfy strong-wolfe-rule
    (if (and ((sdc? 0 0) y0 y0) ((cc? 0) dy0))
        (cons 0 0) ;ok
        (let* ([s1-ext (make-step-allowed step-size 0)]
               [s1 (car s1-ext)])
          (if (cdr s1-ext)   ;;碰到边界
              (cons s1 (max (* 2 sigma) 1.0))
              (let loop ([s0 0] [ys0 y0] [dys0 dy0]
                         [s1 s1] [ys1 (y s1)])
                ;;require: y'(s0) < 0 and s0 < s1
                ;;(display "  --loop--  ")
                (assert (< dys0 0)) (assert (< s0 s1))
                (if ((sdc? s1 s0) ys1 ys0)
                    (let ([dys1 (dy s1)])
                      (cond [((cc? s1) dys1) (cons s1 0)] ;ok
                            [(negative? dys1)
                             (let* ([new-s1-ext (increase-step s1)]
                                    [new-s1 (car new-s1-ext)])
                               (if (cdr new-s1-ext)
                                   (cons new-s1 (max (* 2 sigma) 1.0))
                                   (loop s1 ys1 dys1 new-s1 (y new-s1))))]
                            (else (cons ((zoom s1 s0) ys1 dys1 ys0) 0)))) ;ok
                    (cons ((zoom s0 s1) ys0 dys0 ys1) 0))))))) ;ok
  );;;; end line-search


;;; find direction

                                        ; descent-direction?
(define (descent-direction? d g epsilon)
  (< (/ (vector-dot d g) (vector-norm d) (vector-norm g)) (- epsilon)))

;; (B1) (SD) Steepest-Descent method 
(define (SD g1)
  (scalar-*-vector -1 g1))

;; (B2) (Sub) Subspace method
(define (subspace/with-sd grad constraints f0 ng0 ug0 nd0 ud0 x1 f1 ng1 ug1 min-step)
  ;; grad-function; x0's info:f0,g0; d0=x1-x0; x1's info:f1,g1;
  ;; nv: norm of v, uv: unit-vector of v.
  ;; min-step: minimun-distance between two points(as input).
  ;;;; return: (list nd1 ud1)
  (define (generate-xs)   ;;aux point xs to help constructing Hessian
    (let loop ([s min-step])
      (let ([ds (scalar-*-vector (- s) ug1)]
            [xs (vector-add x1 ds)])
        (if (allowed-point? xs)
            (list s ds xs)
            (loop (* 0.8 s))))))
  (let* ([dots (map vector-dot
                    (list     ud0   ud0  ug0)
                    (list     ug0   ug1  ug1)
                    ;;dots   d0g0  d0g1 g0g1
                    )]
         [s-ds-xs (generate-xs)]
         [s (car s-ds-xs)] [ds (cadr s-ds-xs)] [xs (caddr s-ds-xs)]
         [gs (grad xs)] [ngs (vector-norm gs)] [ugs (scalar-*-vector (/ 1 ngs) gs)]
         [g1-by-d0 (/ ng1 nd0)] [g0-by-g1 (/ ng0 ng1)] [gs-by-g1 (/ ngs ng1)] [g1-by-ds (/ ng1 s)]
         [d0Gd0 (* g1-by-d0 (- (list-ref dots 1) (* g0-by-g1 (list-ref dots 0))))]
         [d0Gg1 (* g1-by-d0 (- 1 (* g0-by-g1 (list-ref dots 2))))]
         [g1Gg1 (* g1-by-ds (- 1 (* gs-by-g1 (vector-dot ug1 ugs))))]
         [is-G-positive? (and (positive? d0Gd0)
                              (positive? (- (* d0Gd0 g1Gg1) (expt d0Gg1 2))))])
    (if is-G-positive?
        (let* ([beta (/ (- (* (list-ref dots 1) g1Gg1) d0Gg1)
                        (- (* (list-ref dots 1) d0Gg1) d0Gd0))]
               [pre-s (* ng1 (/ (- 1 (* beta (list-ref dots 1)))
                                (+ g1Gg1 (* -2 beta d0Gg1) (* beta beta d0Gd0))))]
               [dir (scalar-*-vector pre-s (vector-minus (scalar-*-vector beta ud0) ug1))]
               [ndir (vector-norm dir)] [udir (scalar-*-vector (/ 1 ndir) dir)])
          (if (and (descent-direction? udir ug1) (positive? pre-s))
              (cons (* pre-s ndir) udir) ;;ok
              (begin (printf "use ** sd ** ")
                     (cons s (scalar-*-vector -1 ug1)))));;ok
        (begin (printf "USE ** SD ** ")
               (cons s (scalar-*-vector -1 ug1))))));;ok

;;; Collect line-search-algorithm and find-direction methods
;; SD-method
;; (A) stream form
(define (SD-method-stream f grad constraints grad-constrs x-init epsilon-x epsilon-g c1 c2)
  (define (SD-direction x) (SD (grad x)))
  (define (SD-step-ext x d sigma) ((get-step f grad constraints grad-constrs sigma epsilon-x c1 c2) x d #f))
  (assert (and (< 0 c1) (< c1 c2) (< c2 1)))
  (let loop-stream ([x x-init] [sigma 0])
    (stream-cons x
                 (let* ([d (SD-direction x)]
                        [s-ext (SD-step-ext x d sigma)]
                        [next-x (next-point x (car s-ext) d)])
                   (loop-stream next-x (cdr s-ext))))))


;; (B) ordinary form
(define (SD-method f grad constraints grad-constrs x-init epsilon-x epsilon-g c1 c2)
  (define (SD-direction x) (SD (grad x)))
  (define (SD-step x d sigma) ((get-step f grad constraints grad-constrs sigma epsilon-x c1 c2) x d #f))
  (assert (and (< 0 c1) (< c1 c2) (< c2 1)))
  (let loop ([x x-init] [sigma 0])
    (let* ([d (SD-direction x)]
           [s-ext (SD-step x d sigma)]
           [next-x (next-point x (car s-ext) d)])
      ;;(display (f next-x)) (newline)
      (if (< (vector-norm (grad next-x)) epsilon-g) ;;终止条件
          next-x ;;ok
          (loop next-x (cdr s-ext))))))

;; Subspace method
#|
(define (Subspace/with-SD-method f grad constraints grad-constrs x-init epsilon-x epsilon-g c1 c2)
  (define (SD-direction g) (SD g))
  (define step-function (get-step f grad constraints epsilon-x c1 c2))
  (assert (and (< 0 c1) (< c1 c2) (< c2 1)))
  (let* ([x0 x-init] [g0 (grad x0)]
         [d0 (SD-direction g0)] [s0 (step-function x0 d0 #f)]
         [x1 (next-point x0 s0 d0)] [g1 (grad x1)])    
    (let loop ([x0 x0] [g0 g0]
               [d0 d0] [s0 s0]
               [x1 x1] [g1 g1])
      (if (< (vector-norm g1) epsilon-g) x1 ;;ok
          (let* ([ng0 (vector-norm g0)] [ug0 (scalar-*-vector (/ 1 ng0) g0)]
                 [nd0 (vector-norm d0)] [ud0 (scalar-*-vector (/ 1 nd0) d0)]
                 [sub (subspace/with-sd grad constraints f0 ng0 ug0 nd0 ud0 x1 f1 ng1 ug1 epsilon-x)]
                 [s1 (step-fuction x1 (cdr sub) (car sub))]
                 [x2 (next x1 s1 (cdr sub))])
            (loop x1 g1 (cdr sub) s1 x2 (grad x2))))
      )))
|#


